//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include <ddk.h>
#include <drvpub.h>
#include "ccamera.h"
#include "ov.h"
#include "bulverde.h"

#define LIGHT_FREQ_50   1//fluorescent freq: 50hz//sean
//#define LIGHT_FREQ_60   1//fluorescent freq: 60hz//sean

/*
// System Clock = 26Mhz
//                              Frame Rate
// Light Frequency          60Hz              50Hz
// Light Environment    Normal  Night   Normal  Night
// Preview  QCIF          15    7.5      12.5   6.25
// Preview  QVGA          15    7.5      12.5   6.25
// Capture  QCIF          15    7.5      12.5   6.25
// Capture  QVGA          15    7.5      12.5   6.25
// Capture  VGA           15    7.5      12.5   6.25
// Capture  SXGA          7.5   3.75     6.25   3.125
*/

/////////////////////////////////////////////////////////////////
//
// Sequences
//
/////////////////////////////////////////////////////////////////

// D:0xc6 -> The value that document tells us to set
// C:0x22 -> The value that the co-worker tells us to set
// I:0x88 -> The value that I set

//#define PR3_MODE
static UInt8 gYUV_Init[] =
{
    0x39,   0x43,
    0x38,   0x12,
    0x37,   0x00,
    0x0e,   0x20,

#ifdef PR3_MODE
    0x1e,   0x04,
#else
    0x1e,   0x34,
#endif

    0x01,   0x80,
    0x02,   0x80,
    0x00,   0x00,
    0x10,   0xf0,
    0x04,   0x00,

    0x0c,   0x05,
    0x0d,   0x80,
    0x11,   0x83,
    0x12,   0x10,
    0x14,   0x1a,
    //0x14, 0x2e,

    0x15,   0x22,

    0x18,   0xc8,
    0x17,   0x28,
    0x32,   0xa4,
    0x03,   0x36,
    0x1a,   0x1e,
    0x19,   0x00,
    0x3f,   0xa6,
    0x41,   0x02,
    0x42,   0x08,
    0x1b,   0x00,
    0x16,   0x06,
    0x33,   0xc0,
    0x34,   0xbf,
    0xa8,   0x80,
    0x96,   0x04,

    0x3a,   0x0c,
    0x8e,   0x00,
    0x3c,   0x77,
    0x8b,   0x06,
    0x35,   0x91,
    0x94,   0x88,
    0x95,   0x88,
    0x40,   0xc1,
    0x29,   0x3f,

    0x0f,   0x4a,
    0x27,   0x80,
    0x28,   0x80,
    0x2c,   0x80,
    0x62,   0x00,
    0x63,   0x00,
    0x64,   0x05,
    0x65,   0x00,
    0x66,   0x01,

    0x13,   0xe5,
    0x3d,   0x92,
    0x69,   0x80,

    0x5c,   0x96,
    0x5d,   0x96,
    0x5e,   0x10,
    0x59,   0xeb,
    0x5a,   0x9c,
    0x5b,   0x55,
    0x43,   0xf0,
    0x44,   0x10,
    0x45,   0x55,
    0x46,   0x86,
    0x47,   0x64,
    0x48,   0x86,
    0x5f,   0xe0,
    0x60,   0x8c,
    0x61,   0x20,
    0xa5,   0xd9,

    0xa4,   0x74,
    0x8d,   0x02,
    0x13,   0xe7,
    0x4f,   0x3a,
    0x50,   0x3d,
    0x51,   0x03,
    0x52,   0x12,
    0x53,   0x26,
    0x54,   0x38,
    0x55,   0x40,
    0x56,   0x40,
    0x57,   0x40,
    0x58,   0x0d,
    0x8c,   0x23,
    0x3e,   0x02,
    0xa9,   0xb8,
    0xaa,   0x92,
    0xab,   0x0a,
    0x8f,   0xdf,
    0x90,   0x00,
    0x91,   0x00,
    0x9f,   0x00,

    0x3a,   0x0d,
    0x24,   0x70,
    0x25,   0x64,
    0x26,   0xc3,

    0x3b,   0x09, //add auto night mode; 0xa9 max frame rate reduce to 1/2;0x09 disable this mode
    0x6c,   0x40,
    0x6d,   0x30,
    0x6e,   0x4b,
    0x6f,   0x60,
    0x70,   0x70,
    0x71,   0x70,
    0x72,   0x70,
    0x73,   0x70,
    0x74,   0x60,
    0x75,   0x60,
    0x76,   0x50,
    0x77,   0x48,
    0x78,   0x3a,
    0x79,   0x2e,
    0x7a,   0x28,
    0x7b,   0x22,
    0x7c,   0x04,
    0x7d,   0x07,
    0x7e,   0x10,
    0x7f,   0x28,
    0x80,   0x36,
    0x81,   0x44,
    0x82,   0x52,
    0x83,   0x60,
    0x84,   0x6c,
    0x85,   0x78,
    0x86,   0x8c,
    0x87,   0x9e,
    0x88,   0xbb,
    0x89,   0xd2,
    0x8a,   0xe6,
    0x6a,   0x4b,

    0x2a,   0x00,
    0x2b,   0xd5,

    OV9640_REGEND,  0x00    // End of list delimiter.
};

static UInt8 gYUV_QVGA_1x[] =
{
    // QVGA Preview & Capture, ZOOM 1x
    // Set to QVGA Mode
    0x04,   0x00, // C:0x00 D:Not Set
    0x11,   0x83,
    0x12,   0x10,
    0x0c,   0x05, // COM3 0x05
    0x0d,   0x80,
    0x18,   0xc8, // HSTOP D:0xc6->C:0xc8
    0x17,   0x28, // HSTART D:0x26->C:0x28
    0x32,   0xa4,
    0x03,   0x36,
    0x1a,   0x1e,
    0x19,   0x00,
    // Adjust Frame Rate
#ifdef LIGHT_FREQ_50
    0x2a,   0x10,
    0x2b,   0xe0,
#else
#ifdef LIGHT_FREQ_60
    0x2a,   0x00,
    0x2b,   0x85,
#endif
#endif
    OV9640_REGEND,  0x00        // End of list delimiter.
};
static UInt8 gYUV_QVGA_1x_Light[2][6] =
{
    {
        // Normal
        0x6a,   0x1f,
        OV9640_REGEND,  0x00,
        OV9640_REGEND,  0x00 // ALIGN
    },
    {
        // Night mode
        0x11,   0x87,
        0x6a,   0x1f, // D:0x0f I:0x1f
        OV9640_REGEND,  0x00
    }
};

static UInt8 gYUV_QVGA_2x[] =
{
    // QVGA Preview & Capture, ZOOM 2x

    // Set to VGA Mode
    0x04,   0x00, // C:0x00 D:Not Set
    0x0c,   0x05,
    0x0d,   0x80,
    0x11,   0x81,
    0x12,   0x40,

    // Set Window size to QVGA
    0x18,   0x9e,
    0x17,   0x4e,
    0x32,   0xad,
    0x03,   0x00,
    0x1a,   0x2e,
    0x19,   0x10,

    // Adjust Frame Rate
#ifdef LIGHT_FREQ_50
    0x2a,   0x10, // EXHCH 0x10
    0x2b,   0xe0, // EXHCL 0xe0
#else
#ifdef LIGHT_FREQ_60
    0x2a,   0x00,
    0x2b,   0x85,
#endif
#endif
    OV9640_REGEND,  0x00        // End of list delimiter.
};
static UInt8 gYUV_QVGA_2x_Light[2][6] =
{
    {
        // Normal
        0x6a,   0x3e,
        OV9640_REGEND,  0x00,
        OV9640_REGEND,  0x00 // ALIGN
    },
    {
        // Night mode
        0x11,   0x83,
        0x6a,   0x1f,
        OV9640_REGEND,  0x00
    }
};

static UInt8 gYUV_QVGA_4x[] =
{
    // QVGA Preview & Capture, ZOOM 4x

    // Set to SXGA Mode
    0x04,   0x00,
    0x0c,   0x01,
    0x0d,   0x00,
    0x11,   0x80,
    0x12,   0x00,
    // Set window to QVGA
    0x18,   0x81,
    0x17,   0x59,
    0x32,   0xad,
    0x03,   0x12,
    0x1a,   0x50,
    0x19,   0x32,

    // Adjust Frame Rate
#ifdef LIGHT_FREQ_50
    0x2a,   0x10, // EXHCH 0x10
    0x2b,   0xc8, // EXHCL 0xc8
#else
#ifdef LIGHT_FREQ_60
    0x2a,   0x00,
    0x2b,   0x7f,
#endif
#endif
    OV9640_REGEND,  0x00        // End of list delimiter.
};
static UInt8 gYUV_QVGA_4x_Light[2][6] =
{
    {
        // Normal
        0x6a,   0x41,
        OV9640_REGEND,  0x00,
        OV9640_REGEND,  0x00 // ALIGN
    },
    {
        // Night mode
        0x11,   0x81,
        0x6a,   0x20,
        OV9640_REGEND,  0x00
    }
};

static UInt8 gYUV_VGA_1x[] =
{
    // VGA Capture, ZOOM 1x
    0x04,   0x00, // C:0x00 D:Not Set
    0x0c,   0x05,
    0x0d,   0xc0, // C:
    // Set to VGA Mode
    0x11,   0x81,
    0x12,   0x40,
    0x18,   0xc7, // C:0xc7 D:0xc6
    0x17,   0x27, // C:0x27 D:0x26
    0x32,   0xad, // C:0xbf D:0xad
    0x03,   0x00,
    0x1a,   0x3d,
    0x19,   0x01,

    // Adjust Frame Rate
#ifdef LIGHT_FREQ_50
    0x2a,   0x10, // EXHCH 0x10
    0x2b,   0xe0, // EXHCL 0xe0
#else
#ifdef LIGHT_FREQ_60
    0x2a,   0x00,
    0x2b,   0x85,
#endif
#endif
    OV9640_REGEND,  0x00        // End of list delimiter.
};
static UInt8 gYUV_VGA_1x_Light[2][6] =
{
    {
        // Normal
        OV9640_REGEND,  0x00,
        OV9640_REGEND,  0x00, // ALIGN
        OV9640_REGEND,  0x00 // ALIGN
    },
    {
        // Night mode
        0x11,   0x83,
        OV9640_REGEND,  0x00, // ALIGN
        OV9640_REGEND,  0x00 // ALIGN
    }
};

static UInt8 gYUV_VGA_2x[] =
{
    // VGA Capture, ZOOM 2x

    // Set to SXGA Mode
    0x04,   0x00, // I:
    0x0c,   0x01,
    0x0d,   0x00,
    0x11,   0x80,
    0x12,   0x00,
    0x14,   0x2e,
    0x15,   0x00,
    0x18,   0x95,
    0x17,   0x45,
    0x32,   0xad,
    0x03,   0x12,
    0x1a,   0x5f,
    0x19,   0x23,

    // Adjust Frame Rate
#ifdef LIGHT_FREQ_50
    0x2a,   0x10, // EXHCH 0x10
    0x2b,   0xc8, // EXHCL 0xc8
#else
#ifdef LIGHT_FREQ_60
    0x2a,   0x00,
    0x2b,   0x7f,
#endif
#endif
    OV9640_REGEND,  0x00        // End of list delimiter.
};
static UInt8 gYUV_VGA_2x_Light[2][6] =
{
    {
        // Normal
        OV9640_REGEND,  0x00,
        OV9640_REGEND,  0x00, // ALIGN
        OV9640_REGEND,  0x00 // ALIGN
    },
    {
        // Night mode
        0x11,   0x81,
        OV9640_REGEND,  0x00, // ALIGN
        OV9640_REGEND,  0x00 // ALIGN
    }
};

static UInt8 gYUV_SXGA_1x[] =
{
    // SXGA Capture, ZOOM 1x

    // Set to SXGA Mode
    0x04,   0x00, // C:0x00 D:Not Set
    0x0c,   0x01,
    0x0d,   0x00, // C:0x40
    0x11,   0x80,
    0x12,   0x00,
    0x18,   0xbe, // C:0xbe D:0xbd
    0x17,   0x1e, // C:0x1e D:0x1d
    0x32,   0xad, // C:0x80 D:0xad
    0x03,   0x12,
    0x1a,   0x81,
    0x19,   0x01,

    // Adjust Frame Rate
#ifdef LIGHT_FREQ_50
    0x2a,   0x10, // EXHCH 0x10
    0x2b,   0xc8, // EXHCL 0xc8
#else
#ifdef LIGHT_FREQ_60
    0x2a,   0x00,
    0x2b,   0x7f,
#endif
#endif
    OV9640_REGEND,  0x00        // End of list delimiter.
};
static UInt8 gYUV_SXGA_1x_Light[2][6] =
{
    {
        // Normal
        OV9640_REGEND,  0x00,
        OV9640_REGEND,  0x00, // ALIGN
        OV9640_REGEND,  0x00 // ALIGN
    },
    {
        // Night mode
        0x11,   0x81,
        OV9640_REGEND,  0x00, // ALIGN
        OV9640_REGEND,  0x00 // ALIGN
    }
};

static UInt8 gYUV_QCIF_1x[] =
{
    // QCIF Preview & Capture, ZOOM 1x
    0x04,   0x00, // C:0x00 D:Not Set
    0x11,   0x87,
    0x12,   0x08,
    0x0c,   0x05,
    0x0d,   0xc0,
    0x18,   0x7e,
    0x17,   0x26,
    0x32,   0x24,
    0x03,   0x36,
    0x1a,   0x12,
    0x19,   0x00,

    // Adjust Frame Rate
#ifdef LIGHT_FREQ_50
    0x2a,   0x10, // EXHCH 0x10
    0x2b,   0x38, // EXHCL 0x38
#else
#ifdef LIGHT_FREQ_60
    0x2a,   0x00,
    0x2b,   0x56,
#endif
#endif
    OV9640_REGEND,  0x00        // End of list delimiter.
};
static UInt8 gYUV_QCIF_1x_Light[2][6] =
{
    {
        // Normal
        0x6a,   0x18,
        OV9640_REGEND,  0x00,
        OV9640_REGEND,  0x00 // ALIGN
    },
    {
        // Night mode
        0x11,   0x8f,
        0x6a,   0x0c, // D:0x0C I:0x18
        OV9640_REGEND,  0x00
    }
};

static UInt8 gYUV_QCIF_2x[] =
{
    // QCIF Preview & Capture, ZOOM 2x

    // Set to CIF Mode
    0x04,   0x00, // C:0x00 D:Not Set
    0x11,   0x83,
    0x12,   0x20,
    0x0c,   0x05,
    0x0d,   0xc0,
    0x18,   0x68,
    0x17,   0x3c,
    0x32,   0x24,
    0x03,   0x36,
    0x1a,   0x1b,
    0x19,   0x09,

    // Adjust Frame Rate
#ifdef LIGHT_FREQ_50
    0x2a,   0x10, // EXHCH 0x10
    0x2b,   0x38, // EXHCL 0x38
#else
#ifdef LIGHT_FREQ_60
    0x2a,   0x00,
    0x2b,   0x56,
#endif
#endif
    OV9640_REGEND,  0x00        // End of list delimiter.
};
static UInt8 gYUV_QCIF_2x_Light[2][6] =
{
    {
        // Normal
        0x6a,   0x30,
        OV9640_REGEND,  0x00,
        OV9640_REGEND,  0x00 // ALIGN
    },
    {
        // Night mode
        0x11,   0x87,
        0x6a,   0x18,
        OV9640_REGEND,  0x00
    }
};

static UInt8 gYUV_QCIF_4x[] =
{
    // QCIF Preview & Capture, ZOOM 4x

    0x04,   0x00, // C:0x00 D:Not Set
    0x11,   0x81,
    0x12,   0x40,
    0x0c,   0x05,
    0x0d,   0xc0,
    0x18,   0x8c,
    0x17,   0x60,
    0x32,   0xad,
    0x03,   0x00,
    0x1a,   0x2b,
    0x19,   0x13,

    // Adjust Frame Rate
#ifdef LIGHT_FREQ_50
    0x2a,   0x10, // EXHCH 0x10
    0x2b,   0xe0, // EXHCL 0xe0
#else
#ifdef LIGHT_FREQ_60
    0x2a,   0x00,
    0x2b,   0x85,
#endif
#endif
    OV9640_REGEND,  0x00        // End of list delimiter.
};
static UInt8 gYUV_QCIF_4x_Light[2][6] =
{
    {
        // Normal
        0x6a,   0x3e,
        OV9640_REGEND,  0x00,
        OV9640_REGEND,  0x00 // ALIGN
    },
    {
        // Night mode
        0x11,   0x83,
        0x6a,   0x1f,
        OV9640_REGEND,  0x00
    }
};


/////////////////////////////////////////////////////////////////
//
// Resolutions
//
/////////////////////////////////////////////////////////////////

// Resolutions that the drvier supports
// VideoWidth & VideoHeight: Used for preview & video
// StillWidth & StillHeight: Same to the value of _CamDrv_Resolution_App
static const CamDrv_Resolution s_CamDrv_Resolution[] =
{
    {320, 240, 320,  240},  // QVGA     PHOTO
    {320, 240, 640,  480},  // VGA      PHOTO
    {320, 240, 1280, 960},  // SXGA     PHOTO
    {176, 144, 176,  144},  // QCIF     VIDEO
};

#ifdef DEBUG_CAMERA
EXTERN_C void DumpSensorRegs()
{
    char regValue = 0;
    for(int i = 0; i <= 0xaa; i++)
    {
        I2C_read_byte(i, &regValue);
        cprintf("reg %x: %02x\n", i, 0xff & regValue);
    }
}
#endif

/////////////////////////////////////////////////////////////////
//
// Sensor GPIO
//
/////////////////////////////////////////////////////////////////

EXTERN_C void SensorPowerDown(int powerDown)
{
    //power on.
    GPDR1 |= GPIO_bit(56);

    if( powerDown == 1 ) {

        GPSR1 |= GPIO_bit(56);
    }
    else
        GPCR1 |= GPIO_bit(56);

    DzDelay(50);
}

EXTERN_C void SensorReset()
{
    //reset
    GPSR1 |= GPIO_bit(57);      /* high */
    DzDelay(10);
    GPCR1 |= GPIO_bit(57);      /* low */
}

/////////////////////////////////////////////////////////////////
//
// Sensor Settings
//
/////////////////////////////////////////////////////////////////

static int SensorSetRegs(UInt8 *pReg )
{
    UInt32 CurReg = 0;
    int status = 0;

    // The list is a register number followed by the value.
    while( pReg[CurReg << 1] < OV9640_REGEND )//remarked by sean
    {
        status = I2C_write_byte( pReg[CurReg << 1], pReg[(CurReg <<1)+1] );
        if(CurReg == 0)
            DzDelay(10);
        //cprintf("CurReg %d %x %x\n", CurReg, pReg[CurReg << 1], pReg[(CurReg <<1)+1]);
        CurReg++;
    }
    return status;
}

EXTERN_C ECode SensorInit(PCamDrv_Context camera_context)
{
    // Initialize the hardware
    UInt8 regValue;

    //shutdown view finder and wait for capture
    SensorViewfinderOff();

    regValue = 0x80;
    I2C_write_byte(OV9640_COM7/*0x12*/, regValue); // Init Sequence
    DzDelay(2000);
    I2C_write_byte(OV9640_COM7/*0x12*/, regValue); // Init Sequence
    DzDelay(2000);

    return SensorSetRegs(gYUV_Init);
}

EXTERN_C void SensorViewfinderOn(void)
{
    char    com3;
    I2C_read_byte( OV9640_COM3/*0x0c*/, &com3 );
    com3 &= ~0x01;
    I2C_write_byte( OV9640_COM3/*0x0c*/, com3 );
    return;
}


EXTERN_C void SensorViewfinderOff(void)
{
    char    com3;
    I2C_read_byte( OV9640_COM3/*0x0c*/, &com3 );
    com3 |= 0x01;
    I2C_write_byte( OV9640_COM3/*0x0c*/, com3 );
    return;
}

static void SensorSetStillCapture(PCamDrv_Context pCameraCxt)
{
    SensorSettings * pSS = &pCameraCxt->OmmSettings;
    char aecl_val,aech_val,aechm_val;
    char gain = pSS->field_gain;
    int resLevel = pCameraCxt->StillResLevel;
    int zoom = pCameraCxt->AppFormat.Zoom;
    int exp = pSS->field_exp;

    assert(pCameraCxt->CaptureStatus == CamDrv_Status_Still);

    switch (resLevel) {
        case RES_VGA: // 1x, 2x
            if (zoom == 1) {
                exp = pSS->field_exp * 498 / 248;
            }
            else if (zoom == 2) {
            }
            break;

        case RES_SXGA: // 1x
            exp = pSS->field_exp * 998 / 248 / 2;
            break;

        default:
            break;
    }

    if ((resLevel == RES_VGA && zoom == 2) || resLevel == RES_SXGA){
        if (gain & 0x80) {
            if (exp < 998 / 2) {
                gain &= ~0x80;
                exp = exp * 2;
            }
         }
         if (gain & 0x40) {
            if (exp < 998 / 2) {
                gain &= ~0x40;
                exp = exp * 2;
            }
         }
         if (gain & 0x20) {
            if (exp < 998 / 2) {
                gain &= ~0x20;
                exp = exp * 2;
            }
         }
         if (gain & 0x10) {
            if (exp < 998 / 2) {
                gain &= ~0x10;
                exp = exp * 2;
            }
         }
    }

    // set exposure values
    I2C_read_byte(0x04, &aecl_val);//0x04
    aecl_val &= 0xfc;
    aecl_val |= (char)(exp & 0x03);

    I2C_read_byte(0xa1, &aechm_val);//0xa1
    aechm_val &= 0xc0;
    aechm_val |= (char)((exp & 0xfc00) >> 10);

    aech_val = (char)((exp & 0x3fc) >>2);//0x10

    I2C_write_byte(0x00, gain);//set gain
    I2C_write_byte(0x01, pSS->field_blueg);//set bule gain
    I2C_write_byte(0x02, pSS->field_redg);//set red gain
    I2C_write_byte(0xa1, aechm_val);//set exposure msb 5 bits
    I2C_write_byte(0x10, aech_val);//set exposure high 8 bits
    I2C_write_byte(0x04, aecl_val);//set exposure low 3 bits
}

static void SensorFieldProtect(PCamDrv_Context pCxt)
{
    char gain_val,aechm_val,aech_val,aecl_val,blueg_val,redg_val;
    SensorSettings * pSS = &pCxt->OmmSettings;

    /*save gain*/
    I2C_read_byte(0x00, &gain_val);//gain
    pSS->field_gain = gain_val;
    /*save exposure*/
    I2C_read_byte(0xa1, &aechm_val);//exposure msb 5bit
    I2C_read_byte(0x10, &aech_val);//exposure hsb 8bit
    I2C_read_byte(0x04, &aecl_val);//exposure lsb 3bit
    pSS->field_exp =
        (((UInt16)(aechm_val & 0x3f)) << 10) +
        (((UInt16)(aech_val)) << 2) + (UInt16)(aecl_val & 0x03);

    /*save blue,red*/
    I2C_read_byte(0x01, &blueg_val);//blue gain
    I2C_read_byte(0x02, &redg_val);//red gain
    pSS->field_blueg = blueg_val;
    pSS->field_redg = redg_val;

}

EXTERN_C ECode SensorStartCapture(PCamDrv_Context pCameraCxt)
{
    UInt8 * regsMain = NULL, * regsLight = NULL;
    int resLevel = (pCameraCxt->CaptureStatus == CamDrv_Status_Still)
                ? pCameraCxt->StillResLevel: pCameraCxt->VideoResLevel;
    int zoom = pCameraCxt->AppFormat.Zoom;
    int light = (pCameraCxt->AppFormat.SceneMode == CAMERA_SM_NIGHT)
                ? 1 : 0;

    switch (resLevel) {
        case RES_QVGA: // 1x, 2x, 4x
            if (zoom == 1) {
                regsMain = gYUV_QVGA_1x;
                regsLight = gYUV_QVGA_1x_Light[light];
            }
            else if (zoom == 2) {
                regsMain = gYUV_QVGA_2x;
                regsLight = gYUV_QVGA_2x_Light[light];
            }
            else if (zoom == 4) {
                regsMain = gYUV_QVGA_4x;
                regsLight = gYUV_QVGA_4x_Light[light];
            }
            break;

        case RES_VGA: // 1x, 2x
            if (zoom == 1) {
                regsMain = gYUV_VGA_1x;
                regsLight = gYUV_VGA_1x_Light[light];
            }
            else if (zoom == 2) {
                regsMain = gYUV_VGA_2x;
                regsLight = gYUV_VGA_2x_Light[light];
            }
            break;

        case RES_SXGA: // 1x
            regsMain = gYUV_SXGA_1x;
            regsLight = gYUV_SXGA_1x_Light[light];
            break;

        case RES_QCIF: // 1x, 2x, 4x
            if (zoom == 1) {
                regsMain = gYUV_QCIF_1x;
                regsLight = gYUV_QCIF_1x_Light[light];
            }
            else if (zoom == 2) {
                regsMain = gYUV_QCIF_2x;
                regsLight = gYUV_QCIF_2x_Light[light];
            }
            else if (zoom == 4) {
                regsMain = gYUV_QCIF_4x;
                regsLight = gYUV_QCIF_4x_Light[light];
            }
            break;

        default: // Can't reach here
            // Avoid warning of compiler
            assert(0);
    }

    SensorSetRegs(regsMain); // Set basic resolution sequence
    SensorSetRegs(regsLight); // Set day mode or night mode

    if (pCameraCxt->CaptureStatus == CamDrv_Status_Still) {
        // turn auto function off
        char val;
        I2C_read_byte(OV9640_COM8/*0x13*/, &val );
        val &= ~0x07;
        I2C_write_byte(OV9640_COM8/*0x13*/, val );

        // Set gain & exposure for still mode
        // Actually only for VGA & SXGA
        SensorSetStillCapture(pCameraCxt);
    }
    else {
        // Start AEC/AGC/AWB
        char val;
        I2C_read_byte(OV9640_COM8/*0x13*/, &val );
        val |= 0x07;
        I2C_write_byte(OV9640_COM8/*0x13*/, val );
    }

    SensorViewfinderOn();

    return NOERROR;
}

EXTERN_C void SensorStopCapture(PCamDrv_Context pCameraCxt)
{
    if (pCameraCxt->CaptureStatus == CamDrv_Status_Still) {
        // turn auto function on
        char val;
        I2C_read_byte(OV9640_COM8/*0x13*/, &val );
        val |= 0x07;
        I2C_write_byte(OV9640_COM8/*0x13*/, val );
    }
    else {
        SensorFieldProtect(pCameraCxt);
    }

    // turn viewfinder off
    SensorViewfinderOff();
}

/////////////////////////////////////////////////////////////////
//
// Sensor Functions
//
/////////////////////////////////////////////////////////////////

EXTERN_C ECode SensorSetBrightness(int level)
{
#if 0
    int i;
    Byte reg[11] =
        {0x0f, 0x27, 0x28, 0x2c, 0x62, 0x63, 0x64, 0x65, 0x66, 0x24, 0x25};
    Byte brightness[6][11] =
    {
        {0x4a, 0xa0, 0xa0, 0xa0, 0x00, 0x00, 0x05, 0x00, 0x01, 0x50, 0x44},
        {0x4a, 0x90, 0x90, 0x90, 0x00, 0x00, 0x05, 0x00, 0x01, 0x60, 0x54},
        {0x4a, 0x80, 0x80, 0x80, 0x00, 0x00, 0x05, 0x00, 0x01, 0x70, 0x64},//default
        {0x4a, 0x20, 0x20, 0x20, 0x00, 0x00, 0x03, 0x00, 0x01, 0x80, 0x74},
        {0x4a, 0x40, 0x40, 0x40, 0x00, 0x00, 0x02, 0x00, 0x01, 0x90, 0x84},
        {0x4a, 0x60, 0x60, 0x60, 0x00, 0x00, 0x01, 0x00, 0x01, 0xa0, 0x94},
    };

    DBGOUT(cprintf("SensorSetBright %d\n", level));
    assert (level >= 0 && level <= 5);

    for (i = 0; i < 11; i++) {
        I2C_write_byte(reg[i], brightness[level][i]);
    }
#else
    int i;
    Byte reg[2] = {0x24, 0x25};
    Byte brightness[6][2] =
    {
        {0x50,0x44},
        {0x60,0x54},
        {0x70,0x64}, // default
        {0x90,0x84},
        {0xc0,0xa4},
        {0xe0,0xc4},
    };

    DBGOUT(cprintf("SensorSetBright %d\n", level));
    assert (level >= 0 && level <= 5);

    for (i = 0; i < 2; i++) {
        I2C_write_byte(reg[i], brightness[level][i]);
    }
#endif

    return NOERROR;
}

EXTERN_C ECode SensorSetContrast(int level)
{
    int i;
    Byte reg;
    Byte gamma[7][31] =
    {
      //{0x20,0x50,0x80,0xc0,0xc0,0xa0,0x90,0x78,0x78,0x78,0x40,0x20,0x20,0x20,0x14,0x0e,0x02,0x07,0x17,0x47,0x5f,0x73,0x85,0x94,0xa3,0xb2,0xc2,0xca,0xda,0xea,0xf4},
        {0x02,0x07,0x17,0x47,0x5f,0x73,0x85,0x94,0xa3,0xb2,0xc2,0xca,0xda,0xea,0xf4,0x20,0x50,0x80,0xc0,0xc0,0xa0,0x90,0x78,0x78,0x78,0x40,0x20,0x20,0x20,0x14,0x0e},
        {0x02,0x07,0x1f,0x49,0x5a,0x6a,0x79,0x87,0x94,0x9f,0xaf,0xbb,0xcf,0xdf,0xee,0x20,0x50,0xc0,0xa8,0x88,0x80,0x78,0x70,0x68,0x58,0x40,0x30,0x28,0x20,0x1e,0x17},
        {0x02,0x06,0x16,0x3a,0x4c,0x5c,0x6a,0x78,0x84,0x8e,0x9e,0xae,0xc6,0xda,0xea,0x20,0x40,0x80,0x90,0x90,0x80,0x70,0x70,0x60,0x50,0x40,0x40,0x30,0x28,0x20,0x1c},
        {0x04,0x07,0x10,0x28,0x36,0x44,0x52,0x60,0x6c,0x78,0x8c,0x9e,0xbb,0xd2,0xe6,0x40,0x30,0x48,0x60,0x70,0x70,0x70,0x70,0x60,0x60,0x50,0x48,0x3a,0x2e,0x28,0x22},//default
        {0x04,0x09,0x13,0x29,0x35,0x41,0x4d,0x59,0x64,0x6f,0x85,0x97,0xb7,0xcf,0xe3,0x40,0x50,0x50,0x58,0x60,0x60,0x60,0x60,0x58,0x58,0x58,0x48,0x40,0x30,0x28,0x26},
        {0x05,0x0b,0x16,0x2c,0x37,0x41,0x4b,0x55,0x5f,0x69,0x7c,0x8f,0xb1,0xcf,0xe5,0x50,0x60,0x58,0x58,0x58,0x50,0x50,0x50,0x50,0x50,0x4c,0x4c,0x44,0x3c,0x2c,0x23},
        {0x04,0x09,0x14,0x28,0x32,0x3c,0x46,0x4f,0x58,0x61,0x73,0x85,0xa5,0xc5,0xdd,0x40,0x50,0x58,0x50,0x50,0x50,0x50,0x48,0x48,0x48,0x48,0x48,0x40,0x40,0x30,0x2e}
    };

    DBGOUT(cprintf("SensorSetContrast %d\n", level));
    assert (level >= 0 && level <= 6);

    reg = 0x7c;
    for( i = 0; i < 31; i++, reg++) {
        if(i == 15) reg = 0x6c;
        I2C_write_byte(reg, gamma[level][i]);
    }

    return NOERROR;
}

// TO BE DO MORE...
EXTERN_C ECode SensorSetSharpness(int level)
{
    DBGOUT(cprintf("SensorSetSharpness %d\n", level));

    return E_NOT_IMPLEMENTED;
}

EXTERN_C ECode SensorSetSaturation(int level)
{
    int i;
    Byte reg;
    Byte saturation[5][6] =
    {
        {0x17, 0x18, 0x01, 0x07, 0x0f, 0x16},//weakest
        {0x23, 0x25, 0x02, 0x0b, 0x17, 0x22},
        {0x2e, 0x31, 0x02, 0x0e, 0x1e, 0x2d},
        {0x3a, 0x3d, 0x03, 0x12, 0x26, 0x38},//default
        {0x46, 0x49, 0x04, 0x16, 0x2e, 0x43},//strongest
    };

    DBGOUT(cprintf("SensorSetSaturation %d\n", level));

    reg = 0x4f;
    for(i = 0; i < 6; i++, reg++) {
        I2C_write_byte(reg, saturation[level][i]);
    }

    return NOERROR;
}

EXTERN_C ECode SensorSetWhiteBalance(int WhiteBalance)
{
    Byte tmp;
    Byte banding_switch, gain_max, lens_c5;
    int ret1, ret2, ret3;
//  Byte awb[6][10] =
//  {
//      {0x80, 0x80, 0x20, 0x20, 0x20, 0x20, 0x7a, 0x5d, 0x00, 0x14},//auto white balance
//      {0x80, 0x80, 0x20, 0x20, 0x58, 0x58, 0x7a, 0x5d, 0x00, 0x14},//sunny?
//      {0x80, 0x80, 0x20, 0x20, 0x58, 0x58, 0x7a, 0x5d, 0x00, 0x14},//cloudy?
//      {0x80, 0x80, 0x64, 0x64, 0x58, 0x58, 0x7a, 0x5d, 0x00, 0x14},//fluorescent
//      {0x80, 0x80, 0x64, 0x64, 0x20, 0x20, 0x7a, 0x48, 0x00, 0x14},//incandescent
//      {0x80, 0x80, 0x20, 0x20, 0x20, 0x20, 0x7a, 0x5d, 0x80, 0x94} //night
//  };
    char val[3];
    lens_c5 = 0x01;//reg 0x66

    switch (WhiteBalance) {
    case CAMERA_WB_AUTO:
        gain_max = 2;
        banding_switch = 1;
        SensorSetContrast(3);   // default
        SensorSetSaturation(3); // default
        SensorSetBrightness(2); // default
        break;

    case CAMERA_WB_DAYLIGHT:
        gain_max = 1;
        banding_switch = 0;
        SensorSetContrast(4);
        SensorSetSaturation(4); // strongest
        SensorSetBrightness(2); // default
        break;

    case CAMERA_WB_CLOUDY:
        gain_max = 1;
        banding_switch = 0;
        SensorSetContrast(4);
        SensorSetSaturation(3); // default
        SensorSetBrightness(2); // default
        break;

    case CAMERA_WB_FLUORESCENT:
        gain_max = 2;
        banding_switch = 1;
        SensorSetContrast(3);   // default
        SensorSetSaturation(3); // default
        SensorSetBrightness(2); // default
        break;

    case CAMERA_WB_TUNGSTEN:
        gain_max = 2;
        banding_switch = 1;
        SensorSetContrast(3);   // default
        SensorSetSaturation(2);
        SensorSetBrightness(2); // default
        break;

#if 0
    case CAMERA_WB_NIGHT:
        gain_max = 3;//0x14
        banding_switch = 1; //0x13
        lens_c5 = 0x00;//0x66
        SensorSetContrast(3); // default
        SensorSetSaturation(1);
        SensorSetBrightness(4);
        break;
#endif

    default:
        return E_INVALID_ARGUMENT;
        break;
    }

    ret1 = I2C_read_byte((char)0x13, &val[0]);
    ret2 = I2C_read_byte((char)0x14, &val[1]);
    ret3 = I2C_read_byte((char)0x66, &val[2]);
    if (ret1 == -1 || ret2 == -1 || ret3 == -1)
    {
        DBGOUT(cprintf("Can not read camera sensor register.\n"));
        return E_ACCESS_DENIED;
    }
    tmp = val[0];
    val[0] &= ~(1<<1);//at the start disable awb
    I2C_write_byte(0x13, val[0]);

    val[0] &= ~(1<<5);
    val[1] &= ~(7<<4);
    val[2] &= ~(1<<0);

    val[0] |= (banding_switch<<5);
    val[1] |= (gain_max<<4);
    val[2] |= (lens_c5<<0);
    I2C_write_byte(0x13, val[0]);
    I2C_write_byte(0x14, val[1]);
    I2C_write_byte(0x66, val[2]);

    I2C_read_byte(0x13, &val[0]);
    I2C_read_byte(0x14, &val[1]);
    I2C_read_byte(0x66, &val[2]);

    val[0] |= (1<<1);//test
    I2C_write_byte(0x13, val[0]);//at the end enable awb
    //I2C_write_byte(0x13, tmp);//at the end resume the com8 ?!
    //I2C_write_byte(0x27, b_bias2);//??!!

    return 0;
}

typedef struct
{
    Byte index;
    Byte length;
    Byte *reg;
    Byte *reg_val;
} reg_config_t;

EXTERN_C ECode SensorSetColorMode(int ColorMode)
{
    int i;
    reg_config_t reg_config;

               /*0x4f, 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x13, 0x3a*/
    Byte val0[] =   {0x3a, 0x3d, 0x03, 0x12, 0x26, 0x38, 0x40, 0x40, 0x40, 0x0d, 0xe7, 0x0c};//Normal
    Byte val1[] =   {0x2c, 0x2e, 0x02, 0x0e, 0x1d, 0x2a, 0x40, 0x40, 0x40, 0x0d, 0xe7, 0x0c};//Neutrual
    Byte val2[] =   {0x49, 0x4c, 0x04, 0x17, 0x30, 0x46, 0x40, 0x40, 0x40, 0x0d, 0xe7, 0x0c};//Vivid
               /*0x13, 0x3a, 0x67, 0x68*/
    Byte val3[] =   {0xe7, 0x1c, 0x40, 0xa0}; //Sepia
    Byte val4[] =   {0xe7, 0x1c, 0xa5, 0x65}; //Cold
               /*0x13, 0x3a, 0x67, 0x68*/
    Byte val5[] =   {0xe7, 0x2c, 0x80, 0x80};//Reversal
    Byte val6[] =   {0xe7, 0x1c, 0x80, 0x80}; //B & W
    Byte *reg_val_ary[] = {val0, val1, val2, val3, val4, val5, val6};

    Byte reg0[] =   {0x0c, 0x4f, 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x13, 0x3a};//Normal,Neutrual,Vivid
    Byte reg1[] =   {0x04, 0x13, 0x3a, 0x67, 0x68};//Sepia, Cold
    Byte reg2[] =   {0x04, 0x13, 0x3a, 0x67, 0x68};//Reversal, B & W
    Byte *reg_ary[] = {reg0, reg1, reg2};

    DBGOUT(cprintf("SensorSetColorMode %d\n", ColorMode));

    switch (ColorMode) {
    case CAMERA_CM_COLOR:
        reg_config.reg = reg_ary[0];
        reg_config.reg_val = reg_val_ary[0];
        break;

    case CAMERA_CM_NEUTRAL:
        reg_config.reg = reg_ary[0];
        reg_config.reg_val = reg_val_ary[1];
        break;

    case CAMERA_CM_VIVID:
        reg_config.reg = reg_ary[0];
        reg_config.reg_val = reg_val_ary[2];
        break;

    case CAMERA_CM_SEPIA:
        reg_config.reg = reg_ary[1];
        reg_config.reg_val = reg_val_ary[3];
        break;

    case CAMERA_CM_COOL:
        reg_config.reg = reg_ary[1];
        reg_config.reg_val = reg_val_ary[4];
        break;

    case CAMERA_CM_REVERSE:
        reg_config.reg = reg_ary[2];
        reg_config.reg_val = reg_val_ary[5];
        break;

    case CAMERA_CM_BLACKANDWHITE:
        reg_config.reg = reg_ary[2];
        reg_config.reg_val = reg_val_ary[6];
        break;

    default:
        DBGOUT(cprintf("Invalid camera color mode parameters.\n"));
        return E_INVALID_ARGUMENT;
        break;
    }

    for(i=1; i<= reg_config.reg[0]; i++)
    {
        I2C_write_byte(reg_config.reg[i], reg_config.reg_val[i-1]);
    }

    return NOERROR;
}

// Only support day & night mode now.
EXTERN_C ECode SensorSetSceneMode(int SceneMode)
{
    Byte gain_max, lens_c5, banding_switch;
    int ret1,ret2,ret3;
    char val[3];
    Byte cof_double, edg_enhan;
    int i;
    Byte gamma_ary[2][31] =
    {
        {0x40, 0x30, 0x48, 0x60, 0x70, 0x70, 0x70, 0x70, 0x60, 0x60, 0x50, 0x48, 0x3a, 0x2e, 0x28, 0x22, 0x04, 0x07, 0x10, 0x28, 0x36, 0x44, 0x52, 0x60, 0x6c, 0x78, 0x8c, 0x9e, 0xbb, 0xd2, 0xe6},
        {0x50, 0x60, 0x58, 0x58, 0x58, 0x50, 0x50, 0x50, 0x50, 0x50, 0x4c, 0x4c, 0x44, 0x3c, 0x2c, 0x23, 0x05, 0x0b, 0x16, 0x2c, 0x37, 0x41, 0x4b, 0x55, 0x5f, 0x69, 0x7c, 0x8f, 0xb1, 0xcf, 0xe5}
    };
    Byte *gamma_p;

    DBGOUT(cprintf("SensorSetSceneMode %d\n", SceneMode));
    switch(SceneMode)
    {
        case CAMERA_SM_AUTO:
        case CAMERA_SM_PORTRAIT:
        case CAMERA_SM_SPORT:
        case CAMERA_SM_LANDSCAPE:
        case CAMERA_SM_CLOSEUP:
            gain_max = 2;       //0x14
            banding_switch = 1;     //0x13        // ???
            lens_c5 = 0x01;     //0x66
            cof_double = 0x02;  //0x41
            edg_enhan = 0xa6;   //0x3f
            gamma_p = gamma_ary[0]; //0x6c->0x8a
            SensorSetSaturation(3);//saturation
            break;

        case CAMERA_SM_NIGHT:
            gain_max = 3;       //0x14
            banding_switch = 0;     //0x13          // ???
            lens_c5 = 0x00;     //0x66
            cof_double = 0x02;  //0x41
            edg_enhan = 0xf0;   //0x3f
            gamma_p = gamma_ary[1]; //0x6c->0x8a
            SensorSetSaturation(2);//saturation2
            break;

        default:
            return E_INVALID_ARGUMENT;
    }

    ret1 = I2C_read_byte((char)0x13, &val[0]);
    ret2 = I2C_read_byte((char)0x14, &val[1]);
    ret3 = I2C_read_byte((char)0x66, &val[2]);
    if (ret1 == -1 || ret2 == -1 || ret3 == -1)
    {
        DBGOUT(cprintf("Can not read camera sensor register.\n"));
        return E_ACCESS_DENIED;
    }

    val[0] &= ~(1<<5);
    val[1] &= ~(7<<4);
    val[2] &= ~(1<<0);

    val[0] |= (banding_switch<<5);
    val[1] |= (gain_max<<4);
    val[2] |= (lens_c5<<0);

    //I2C_write_byte(0x13, &val[0]);

    I2C_write_byte(0x14, val[1]);
    I2C_write_byte(0x66, val[2]);
    I2C_write_byte(0x41, cof_double);
    I2C_write_byte(0x3f, edg_enhan);

    for(i= 0; i<=30; i++)
        I2C_write_byte(0x6c + i, gamma_p[i]);

    return NOERROR;
}

EXTERN_C ECode SensorSetExposureCompensation(int ExposureCompensation)
{
    char aew,aeb,vpt;

    switch (ExposureCompensation) {
        case 3:
            aew = 0xb8;
            aeb = 0xb0;
            vpt = 0xe5;
            break;
        case 2:
            aew = 0xa0;
            aeb = 0x98;
            vpt = 0xe5;
            break;
        case 1:
            aew = 0x88;
            aeb = 0x80;
            vpt = 0xd4;
            break;
        case 0:
            aew = 0x78;
            aeb = 0x70;
            vpt = 0xd3;
            break;
        case -1:
            aew = 0x64;
            aeb = 0x5c;
            vpt = 0xc3;
            break;
        case -2:
            aew = 0x58;
            aeb = 0x50;
            vpt = 0xc2;
            break;
        case -3:
            aew = 0x4c;
            aeb = 0x44;
            vpt = 0xb2;
            break;
        default:
            return E_INVALID_ARGUMENT;
            break;
    }
    I2C_write_byte(0x24, aew);
    I2C_write_byte(0x25, aeb);
    I2C_write_byte(0x26, vpt);
    return 0;
}

/////////////////////////////////////////////////////////////////
//
// Sensor Properties
//
/////////////////////////////////////////////////////////////////

// Get the resolution which the driver uses
EXTERN_C ECode SensorGetResolution(PCamDrv_Context pCameraCxt, int resolution)
{
    unsigned int index = (unsigned int)resolution;

    if (index >=
        sizeof(s_CamDrv_Resolution) / sizeof(CamDrv_Resolution)) {
        //DBGOUT(cprintf(("Unsupported photo quality value.\n"));
        return E_INVALID_ARGUMENT;
    }
    memcpy(&pCameraCxt->Resolution, &s_CamDrv_Resolution[index],
        sizeof(CamDrv_Resolution));

    if (pCameraCxt->bVideo) pCameraCxt->VideoResLevel = resolution; // QCIF, QVGA
    else {
        pCameraCxt->StillResLevel = resolution; // QVGA, VGA, SXGA
        pCameraCxt->VideoResLevel = RES_QVGA; // PREVIEW FOR QVGA, VGA, SXGA
    }

    return NOERROR;
}

EXTERN_C ECode SensorCheckDigitalZoom(PCamDrv_Context pCameraCxt, int zoom)
{
    // Correct zoom check
    if (pCameraCxt->bVideo) {
        // VIDEO MODE
        // QCIF 1x, 2x, 4x
        // QVGA 1x, 2x, 4x
        if (pCameraCxt->VideoResLevel == RES_QCIF) {
            if (zoom == 1 || zoom == 2 || zoom == 4) goto CanSet;
        }
        else if (pCameraCxt->VideoResLevel == RES_QVGA) {
            if (zoom == 1 || zoom == 2 || zoom == 4) goto CanSet;
        }
    }
    else {
        // PHOTO MODE
        // QVGA 1x, 2x, 4x
        // VGA  1x, 2x
        // SXGA 1x
        if (pCameraCxt->StillResLevel == RES_QVGA) {
            if (zoom == 1 || zoom == 2 || zoom == 4) goto CanSet;
        }
        else if (pCameraCxt->StillResLevel == RES_VGA) {
            if (zoom == 1 || zoom == 2) goto CanSet;
        }
        else if (pCameraCxt->StillResLevel == RES_SXGA) {
            if (zoom == 1) goto CanSet;
        }
    }

    return E_INVALID_ARGUMENT; // Incorrect zoom

CanSet:
    return NOERROR;
}
